#pragma once
#include "commander/px4_custom_mode.h"
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/test_mavlink_rx.h>
#include <uORB/topics/follow_target.h>
#include <geo/geo.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/a02_dist_to_target.h>
#include <uORB/topics/a03_buoy_sate.h>
#include <lib/drivers/device/Device.hpp>
#include <fcntl.h>
#include <termios.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;

extern "C" __EXPORT int sonar_driver_main(int argc, char *argv[]);


class SonarDriver : public ModuleBase<SonarDriver>, public ModuleParams
{
public:
	SonarDriver(int example_param, bool example_flag);

	virtual ~SonarDriver() = default;

	/** @see ModuleBase */
	static int task_spawn(int argc, char *argv[]);

	/** @see ModuleBase */
	static SonarDriver *instantiate(int argc, char *argv[]);

	/** @see ModuleBase */
	static int custom_command(int argc, char *argv[]);

	/** @see ModuleBase */
	static int print_usage(const char *reason = nullptr);

	/** @see ModuleBase::run() */
	void run() override;

	/** @see ModuleBase::print_status() */
	int print_status() override;

private:

	/**
	 * Check for parameter changes and update them if needed.
	 * @param parameter_update_sub uorb subscription to parameter_update
	 * @param force for a parameter update
	 */
	void parameters_update(bool force = false);
int _fd{-1};
	char _linebuf[10] {};
int init();

	DEFINE_PARAMETERS(
		(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */
		(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */

	)


	// Subscriptions
	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
};

